#!/usr/bin/env python
# vim:set ts=4 sw=4 et:
#
# Copyright 2014 Vladimir Ermakov.
#
# This file is part of the mavros package and subject to the license terms
# in the top-level LICENSE file of the mavros repository.
# https://github.com/mavlink/mavros/tree/master/LICENSE.md

from __future__ import print_function

import argparse

import rospy
import mavros
from mavros.utils import *
from geometry_msgs.msg import PolygonStamped, Point32
from mavros import command


def _arm(args, state):
    try:
        ret = command.arming(value=state)
    except rospy.ServiceException as ex:
        fault(ex)

    if not ret.success:
        fault("Request failed. Check mavros logs")

    print_if(args.verbose, "Command result:", ret.result)
    return ret


def do_arm(args):
    _arm(args, True)


def do_disarm(args):
    _arm(args, False)

def do_kill(args):
    try:
        ret = command.long(command=400, param2=21196)
    except rospy.ServiceException as ex:
        fault(ex)

    if not ret.success:
        fault("Request failed. Check mavros logs")

    print_if(args.verbose, "Command result:", ret.result)
    return ret

_ONCE_DELAY = 3
def do_safetyarea(args):
    set_topic = mavros.get_topic('safety_area', 'set')
    pub = rospy.Publisher(set_topic, PolygonStamped,
                          queue_size=10, latch=True)

    poly = PolygonStamped()
    poly.header.frame_id = 'mavsafety'
    poly.header.stamp = rospy.get_rostime()
    poly.polygon.points = [
        Point32(x=args.p1[0], y=args.p1[1], z=args.p1[2]),
        Point32(x=args.p2[0], y=args.p2[1], z=args.p2[2]),
    ]

    pub.publish(poly)
    # XXX maybe not worked
    print_if(pub.get_num_connections() < 1,
             "Mavros not started, nobody subcsribes to ", set_topic)

    # stick around long enough for others to grab
    timeout_t = rospy.get_time() + _ONCE_DELAY
    while not rospy.is_shutdown() and rospy.get_time() < timeout_t:
        rospy.sleep(0.2)


def main():
    parser = argparse.ArgumentParser(description="Command line tool for manipulating safety on MAVLink device.")
    parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default=mavros.DEFAULT_NAMESPACE)
    parser.add_argument('-v', '--verbose', action='store_true', help="Verbose output")
    subarg = parser.add_subparsers()

    arm_args = subarg.add_parser('arm', help="Arm motors")
    arm_args.set_defaults(func=do_arm)

    disarm_args = subarg.add_parser('disarm', help="Disarm motors")
    disarm_args.set_defaults(func=do_disarm)

    kill_args = subarg.add_parser('kill', help="Kill motors")
    kill_args.set_defaults(func=do_kill)

    safety_area_args = subarg.add_parser('safetyarea', help="Send safety area")
    safety_area_args.set_defaults(func=do_safetyarea)
    safety_area_args.add_argument('-p1', type=float, nargs=3, metavar=('x', 'y', 'z'),
                                  required=True, help="Corner 1")
    safety_area_args.add_argument('-p2', type=float, nargs=3, metavar=('x', 'y', 'z'),
                                  required=True, help="Corner 2")

    args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:])

    rospy.init_node("mavsafety", anonymous=True)
    mavros.set_namespace(args.mavros_ns)
    args.func(args)


if __name__ == '__main__':
    main()
